package behavior;

import lejos.nxt.SensorPort;
import lejos.nxt.TouchSensor;
import lejos.nxt.UltrasonicSensor;
import lejos.robotics.subsumption.Behavior;
import controller.NxtSafeState;

public class Emergency implements Behavior {
	private UltrasonicSensor sonar, sonar2;
	private TouchSensor touch;
	private boolean suppressed = false;

	public Emergency(SensorPort port1, SensorPort port2, SensorPort port3) {
		sonar = new UltrasonicSensor(port1);
		sonar2 = new UltrasonicSensor(port2);
		touch = new TouchSensor(port3);
	}

	public boolean takeControl() {
		try {
			Thread.sleep(50);
		} catch (InterruptedException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
		}
		return (!(sonar.getProductID().toString().equals("Sonar"))
				|| !(sonar2.getProductID().toString().equals("Sonar")) || (touch
					.isPressed()));
	}

	public void suppress() {
		suppressed = true;
	}

	public void action() {
		suppressed = false;
		NxtSafeState.getInstance().putInSafeState();
	}
}
